#ifndef SAFETY_RECHECK_ITF_H
#define SAFETY_RECHECK_ITF_H

#include <math.h>
#include <stdio.h>
#include "zuc/nml_intf/zucpos.h"
#include "motion/safezone.h"
#include "rtapi/rtapi.h"

#ifdef __cplusplus
extern "C" {
#endif

typedef struct
{
    int conical_tip_choice;
    double tool_properties_tilt;
    double tool_properties_pan;
    double deviation;
    double deviation_warning;
    double conical_cenrpy[3];
} ToolOriCons;

typedef struct SafetyConfigTest
{
    double max_pos_limit[ZUCMOT_MAX_JOINTS];
    double min_pos_limit[ZUCMOT_MAX_JOINTS];
    double jvel_limit[ZUCMOT_MAX_JOINTS];
    double tcpVelLimit;
    double stoppingDist;
    double stoppingTime;

    double elbow_sphere_radius;

    double elbow_vel_limit_reduce;
    double tcp_vel_reduce;
    double stopping_time_reduce;
    double stopping_dist_reduce;

    SafeZone zone;
    ToolOriCons tool_ori_cons;
} SafetyConfigTest;

extern int check_jpos_limit(double current_jpos_fb, double max_jpos_limit, double min_jpos_limit);

extern int check_jvel_limit(double current_jvel, double max_jvel_limit);

extern int check_linear_vel_limit(double tcp_vel, double tcp_vel_limit);  // tcp and elbow

extern int check_safe_plane(SafePlane plane, ZucPose* curPose, double* dist, int elbow_limit_flag, double r_elbow);

extern int check_tool_ori(ZucPose* cart_pos, ZucPose* cur_tool_offsets, ToolOriCons* tool_ori_cons, double* incl_angle);

#ifdef __cplusplus
}
#endif

#endif